﻿namespace Pathfinding
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.InteropServices;
    using UnityEngine;

    [RequireComponent(typeof(CharacterController))]
    public class LocalAvoidance : MonoBehaviour
    {
        protected LocalAvoidance[] agents;
        public float circlePoint = 0.5f;
        public float circleScale = 0.5f;
        protected CharacterController controller;
        public float delta = 1f;
        public bool drawGizmos;
        public float maxSpeedScale = 1.5f;
        private const int maxVOCounter = 50;
        private Vector3 preVelocity;
        public const float Rad2Deg = 57.29578f;
        public float radius = 0.5f;
        public float responability = 0.5f;
        public ResolutionType resType = ResolutionType.Geometric;
        public Vector3[] samples;
        public float sampleScale = 1f;
        public float speed = 2f;
        private Vector3 velocity;
        private List<VO> vos = new List<VO>();

        public bool CheckSample(Vector3 sample, List<VO> vos)
        {
            bool flag = false;
            for (int i = 0; i < vos.Count; i++)
            {
                if (vos[i].Contains(sample))
                {
                    if (this.drawGizmos)
                    {
                        Debug.DrawRay(sample, Vector3.up, Color.red);
                    }
                    flag = true;
                    break;
                }
            }
            if (this.drawGizmos && !flag)
            {
                Debug.DrawRay(sample, Vector3.up, Color.yellow);
            }
            return !flag;
        }

        public Vector3 ClampMovement(Vector3 direction)
        {
            Vector3 vector = (Vector3) (direction * this.delta);
            Vector3 target = base.transform.position + direction;
            Vector3 p = target;
            float positiveInfinity = 0f;
            int num2 = 0;
            this.vos.Clear();
            float magnitude = this.velocity.magnitude;
            foreach (LocalAvoidance avoidance in this.agents)
            {
                if ((avoidance != this) && (avoidance != null))
                {
                    Vector3 vector4 = avoidance.transform.position - base.transform.position;
                    float num5 = vector4.magnitude;
                    float num6 = this.radius + avoidance.radius;
                    if ((num5 <= ((((vector.magnitude * this.delta) + num6) + magnitude) + avoidance.GetVelocity().magnitude)) && (num2 <= 50))
                    {
                        num2++;
                        VO item = new VO {
                            origin = base.transform.position + Vector3.Lerp((Vector3) (this.velocity * this.delta), (Vector3) (avoidance.GetVelocity() * this.delta), this.responability),
                            direction = vector4.normalized
                        };
                        if (num6 > vector4.magnitude)
                        {
                            item.angle = 1.570796f;
                        }
                        else
                        {
                            item.angle = (float) Math.Asin((double) (num6 / num5));
                        }
                        item.limit = num5 - num6;
                        if (item.limit < 0f)
                        {
                            item.origin += (Vector3) (item.direction * item.limit);
                            item.limit = 0f;
                        }
                        float num7 = Mathf.Atan2(item.direction.z, item.direction.x);
                        item.pRight = new Vector3(Mathf.Cos(num7 + item.angle), 0f, Mathf.Sin(num7 + item.angle));
                        item.pLeft = new Vector3(Mathf.Cos(num7 - item.angle), 0f, Mathf.Sin(num7 - item.angle));
                        item.nLeft = new Vector3(Mathf.Cos((num7 + item.angle) - 1.570796f), 0f, Mathf.Sin((num7 + item.angle) - 1.570796f));
                        item.nRight = new Vector3(Mathf.Cos((num7 - item.angle) + 1.570796f), 0f, Mathf.Sin((num7 - item.angle) + 1.570796f));
                        this.vos.Add(item);
                    }
                }
            }
            if (this.resType != ResolutionType.Geometric)
            {
                if (this.resType == ResolutionType.Sampled)
                {
                    Vector3 vector11 = vector;
                    Vector3 normalized = vector11.normalized;
                    Vector3 vector13 = Vector3.Cross(normalized, Vector3.up);
                    int num22 = 10;
                    int num23 = 0;
                    while (num23 < 10)
                    {
                        float num24 = (float) ((3.1415926535897931 * this.circlePoint) / ((double) num22));
                        float num25 = ((float) (3.1415926535897931 - (this.circlePoint * 3.1415926535897931))) * 0.5f;
                        for (int num26 = 0; num26 < num22; num26++)
                        {
                            float num27 = num24 * num26;
                            Vector3 sample = (base.transform.position + vector) - ((Vector3) ((((vector11 * ((float) Math.Sin((double) (num27 + num25)))) * num23) * this.circleScale) + (((vector13 * ((float) Math.Cos((double) (num27 + num25)))) * num23) * this.circleScale)));
                            if (this.CheckSample(sample, this.vos))
                            {
                                return (sample - base.transform.position);
                            }
                        }
                        num23++;
                        num22 += 2;
                    }
                    for (int n = 0; n < this.samples.Length; n++)
                    {
                        Vector3 vector15 = (Vector3) (((base.transform.position + (this.samples[n].x * vector13)) + (this.samples[n].z * normalized)) + (this.samples[n].y * vector11));
                        if (this.CheckSample(vector15, this.vos))
                        {
                            return (vector15 - base.transform.position);
                        }
                    }
                }
                return Vector3.zero;
            }
            for (int i = 0; i < this.vos.Count; i++)
            {
                if (this.vos[i].Contains(p))
                {
                    positiveInfinity = float.PositiveInfinity;
                    if (this.drawGizmos)
                    {
                        Debug.DrawRay(p, Vector3.down, Color.red);
                    }
                    p = base.transform.position;
                    break;
                }
            }
            if (this.drawGizmos)
            {
                for (int num9 = 0; num9 < this.vos.Count; num9++)
                {
                    this.vos[num9].Draw(Color.black);
                }
            }
            if (positiveInfinity == 0f)
            {
                return vector;
            }
            List<VOLine> list = new List<VOLine>();
            for (int j = 0; j < this.vos.Count; j++)
            {
                VO vo = this.vos[j];
                float num11 = (float) Math.Atan2((double) vo.direction.z, (double) vo.direction.x);
                Vector3 start = vo.origin + ((Vector3) (new Vector3((float) Math.Cos((double) (num11 + vo.angle)), 0f, (float) Math.Sin((double) (num11 + vo.angle))) * vo.limit));
                Vector3 vector6 = vo.origin + ((Vector3) (new Vector3((float) Math.Cos((double) (num11 - vo.angle)), 0f, (float) Math.Sin((double) (num11 - vo.angle))) * vo.limit));
                Vector3 end = start + ((Vector3) (new Vector3((float) Math.Cos((double) (num11 + vo.angle)), 0f, (float) Math.Sin((double) (num11 + vo.angle))) * 100f));
                Vector3 vector8 = vector6 + ((Vector3) (new Vector3((float) Math.Cos((double) (num11 - vo.angle)), 0f, (float) Math.Sin((double) (num11 - vo.angle))) * 100f));
                int num12 = !Polygon.Left(vo.origin, vo.origin + vo.direction, base.transform.position + this.velocity) ? 2 : 1;
                list.Add(new VOLine(vo, start, end, true, 1, num12 == 1));
                list.Add(new VOLine(vo, vector6, vector8, true, 2, num12 == 2));
                list.Add(new VOLine(vo, start, vector6, false, 3, false));
                bool inside = false;
                bool flag2 = false;
                if (!inside)
                {
                    for (int num13 = 0; num13 < this.vos.Count; num13++)
                    {
                        if ((num13 != j) && this.vos[num13].Contains(start))
                        {
                            inside = true;
                            break;
                        }
                    }
                }
                if (!flag2)
                {
                    for (int num14 = 0; num14 < this.vos.Count; num14++)
                    {
                        if ((num14 != j) && this.vos[num14].Contains(vector6))
                        {
                            flag2 = true;
                            break;
                        }
                    }
                }
                vo.AddInt(0f, inside, 1);
                vo.AddInt(0f, flag2, 2);
                vo.AddInt(0f, inside, 3);
                vo.AddInt(1f, flag2, 3);
            }
            for (int k = 0; k < list.Count; k++)
            {
                for (int num16 = k + 1; num16 < list.Count; num16++)
                {
                    float num17;
                    float num18;
                    VOLine line = list[k];
                    VOLine line2 = list[num16];
                    if (((line.vo != line2.vo) && (((Polygon.IntersectionFactor(line.start, line.end, line2.start, line2.end, out num17, out num18) && (num17 >= 0f)) && (num18 >= 0f)) && (line.inf || (num17 <= 1f)))) && (line2.inf || (num18 <= 1f)))
                    {
                        Vector3 vector9 = line.start + ((Vector3) ((line.end - line.start) * num17));
                        bool flag3 = line.wrongSide || line2.wrongSide;
                        if (!flag3)
                        {
                            for (int num19 = 0; num19 < this.vos.Count; num19++)
                            {
                                if (((this.vos[num19] != line.vo) && (this.vos[num19] != line2.vo)) && this.vos[num19].Contains(vector9))
                                {
                                    flag3 = true;
                                    break;
                                }
                            }
                        }
                        line.vo.AddInt(num17, flag3, line.id);
                        line2.vo.AddInt(num18, flag3, line2.id);
                        if (this.drawGizmos)
                        {
                            Debug.DrawRay(line.start + ((Vector3) ((line.end - line.start) * num17)), Vector3.up, !flag3 ? Color.green : Color.magenta);
                        }
                    }
                }
            }
            for (int m = 0; m < this.vos.Count; m++)
            {
                Vector3 vector10;
                if (this.vos[m].FinalInts(target, base.transform.position + this.velocity, this.drawGizmos, out vector10))
                {
                    Vector3 vector17 = vector10 - target;
                    float sqrMagnitude = vector17.sqrMagnitude;
                    if (sqrMagnitude < positiveInfinity)
                    {
                        p = vector10;
                        positiveInfinity = sqrMagnitude;
                        if (this.drawGizmos)
                        {
                            Debug.DrawLine(target + Vector3.up, p + Vector3.up, Color.red);
                        }
                    }
                }
            }
            if (this.drawGizmos)
            {
                Debug.DrawLine(target + Vector3.up, p + Vector3.up, Color.red);
            }
            return Vector3.ClampMagnitude(p - base.transform.position, vector.magnitude * this.maxSpeedScale);
        }

        public Vector3 GetVelocity()
        {
            return this.preVelocity;
        }

        public void LateUpdate()
        {
            this.preVelocity = this.velocity;
        }

        public void SimpleMove(Vector3 desiredMovement)
        {
            Vector3 vector = (Vector3) (UnityEngine.Random.insideUnitSphere * 0.1f);
            vector.y = 0f;
            Vector3 dir = this.ClampMovement(desiredMovement + vector);
            if (dir != Vector3.zero)
            {
                dir = (Vector3) (dir / this.delta);
            }
            if (this.drawGizmos)
            {
                Debug.DrawRay(base.transform.position, desiredMovement, Color.magenta);
                Debug.DrawRay(base.transform.position, dir, Color.yellow);
                Debug.DrawRay(base.transform.position + dir, Vector3.up, Color.yellow);
            }
            this.controller.SimpleMove(dir);
            this.velocity = this.controller.velocity;
            Debug.DrawRay(base.transform.position, this.velocity, Color.blue);
        }

        private void Start()
        {
            this.controller = base.GetComponent<CharacterController>();
            this.agents = UnityEngine.Object.FindObjectsOfType(typeof(LocalAvoidance)) as LocalAvoidance[];
        }

        public void Update()
        {
            this.SimpleMove((Vector3) (base.transform.forward * this.speed));
        }

        public class HalfPlane
        {
            public Vector3 normal;
            public Vector3 point;

            public Vector3 ClosestPoint(Vector3 p)
            {
                p -= this.point;
                Vector3 lhs = Vector3.Cross(this.normal, Vector3.up);
                float num = Vector3.Dot(lhs, p);
                return (this.point + ((Vector3) (lhs * num)));
            }

            public Vector3 ClosestPoint(Vector3 p, float minX, float maxX)
            {
                p -= this.point;
                Vector3 lhs = Vector3.Cross(this.normal, Vector3.up);
                if (lhs.x < 0f)
                {
                    lhs = -lhs;
                }
                float num = Vector3.Dot(lhs, p);
                float min = (minX - this.point.x) / lhs.x;
                float max = (maxX - this.point.x) / lhs.x;
                num = Mathf.Clamp(num, min, max);
                return (this.point + ((Vector3) (lhs * num)));
            }

            public bool Contains(Vector3 p)
            {
                p -= this.point;
                return (Vector3.Dot(this.normal, p) >= 0f);
            }

            public void Draw()
            {
                Vector3 vector = Vector3.Cross(this.normal, Vector3.up);
                Debug.DrawLine(this.point - ((Vector3) (vector * 10f)), this.point + ((Vector3) (vector * 10f)), Color.blue);
                Debug.DrawRay(this.point, this.normal, new Color(0.8f, 0.1f, 0.2f));
            }

            public void DrawBounds(float left, float right)
            {
                Vector3 vector = Vector3.Cross(this.normal, Vector3.up);
                if (vector.x < 0f)
                {
                    vector = -vector;
                }
                float num = (left - this.point.x) / vector.x;
                float num2 = (right - this.point.x) / vector.x;
                Debug.DrawLine((Vector3) ((this.point + (vector * num)) + (Vector3.up * 0.1f)), (Vector3) ((this.point + (vector * num2)) + (Vector3.up * 0.1f)), Color.yellow);
            }

            public Vector3 Intersection(LocalAvoidance.HalfPlane hp)
            {
                Vector3 vector = Vector3.Cross(this.normal, Vector3.up);
                Vector3 vector2 = Vector3.Cross(hp.normal, Vector3.up);
                return Polygon.IntersectionPointOptimized(this.point, vector, hp.point, vector2);
            }
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct IntersectionPair : IComparable<LocalAvoidance.IntersectionPair>
        {
            public float factor;
            public LocalAvoidance.IntersectionState state;
            public IntersectionPair(float factor, bool inside)
            {
                this.factor = factor;
                this.state = !inside ? LocalAvoidance.IntersectionState.Outside : LocalAvoidance.IntersectionState.Inside;
            }

            public void SetState(LocalAvoidance.IntersectionState s)
            {
                this.state = s;
            }

            public int CompareTo(LocalAvoidance.IntersectionPair o)
            {
                if (o.factor < this.factor)
                {
                    return 1;
                }
                if (o.factor > this.factor)
                {
                    return -1;
                }
                return 0;
            }
        }

        public enum IntersectionState
        {
            Inside,
            Outside,
            Enter,
            Exit
        }

        public enum ResolutionType
        {
            Sampled,
            Geometric
        }

        public class VO
        {
            public float angle;
            public Vector3 direction;
            public List<LocalAvoidance.IntersectionPair> ints1 = new List<LocalAvoidance.IntersectionPair>();
            public List<LocalAvoidance.IntersectionPair> ints2 = new List<LocalAvoidance.IntersectionPair>();
            public List<LocalAvoidance.IntersectionPair> ints3 = new List<LocalAvoidance.IntersectionPair>();
            public float limit;
            public Vector3 nLeft;
            public Vector3 nRight;
            public Vector3 origin;
            public Vector3 pLeft;
            public Vector3 pRight;

            public void AddInt(float factor, bool inside, int id)
            {
                switch (id)
                {
                    case 1:
                        this.ints1.Add(new LocalAvoidance.IntersectionPair(factor, inside));
                        break;

                    case 2:
                        this.ints2.Add(new LocalAvoidance.IntersectionPair(factor, inside));
                        break;

                    case 3:
                        this.ints3.Add(new LocalAvoidance.IntersectionPair(factor, inside));
                        break;
                }
            }

            public bool Contains(Vector3 p)
            {
                return (((Vector3.Dot(this.nLeft, p - this.origin) > 0f) && (Vector3.Dot(this.nRight, p - this.origin) > 0f)) && (Vector3.Dot(this.direction, p - this.origin) > this.limit));
            }

            public void Draw(Color c)
            {
                float num = (float) Math.Atan2((double) this.direction.z, (double) this.direction.x);
                Vector3 vector = (Vector3) ((Vector3.Cross(this.direction, Vector3.up) * ((float) Math.Tan((double) this.angle))) * this.limit);
                Debug.DrawLine((this.origin + ((Vector3) (this.direction * this.limit))) + vector, (this.origin + ((Vector3) (this.direction * this.limit))) - vector, c);
                Debug.DrawRay((this.origin + ((Vector3) (this.direction * this.limit))) + vector, (Vector3) (new Vector3((float) Math.Cos((double) (num + this.angle)), 0f, (float) Math.Sin((double) (num + this.angle))) * 10f), c);
                Debug.DrawRay((this.origin + ((Vector3) (this.direction * this.limit))) - vector, (Vector3) (new Vector3((float) Math.Cos((double) (num - this.angle)), 0f, (float) Math.Sin((double) (num - this.angle))) * 10f), c);
            }

            public bool FinalInts(Vector3 target, Vector3 closeEdgeConstraint, bool drawGizmos, out Vector3 closest)
            {
                this.ints1.Sort();
                this.ints2.Sort();
                this.ints3.Sort();
                float num = (float) Math.Atan2((double) this.direction.z, (double) this.direction.x);
                Vector3 rhs = Vector3.Cross(this.direction, Vector3.up);
                Vector3 vector2 = (Vector3) ((rhs * ((float) Math.Tan((double) this.angle))) * this.limit);
                Vector3 vector3 = (this.origin + ((Vector3) (this.direction * this.limit))) + vector2;
                Vector3 vector4 = (this.origin + ((Vector3) (this.direction * this.limit))) - vector2;
                Vector3 vector5 = vector3 + ((Vector3) (new Vector3((float) Math.Cos((double) (num + this.angle)), 0f, (float) Math.Sin((double) (num + this.angle))) * 100f));
                Vector3 vector6 = vector4 + ((Vector3) (new Vector3((float) Math.Cos((double) (num - this.angle)), 0f, (float) Math.Sin((double) (num - this.angle))) * 100f));
                bool flag = false;
                closest = Vector3.zero;
                int num2 = (Vector3.Dot(closeEdgeConstraint - this.origin, rhs) <= 0f) ? 1 : 2;
                for (int i = 1; i <= 3; i++)
                {
                    if (i == num2)
                    {
                        continue;
                    }
                    List<LocalAvoidance.IntersectionPair> list = (i != 1) ? ((i != 2) ? this.ints3 : this.ints2) : this.ints1;
                    Vector3 lineStart = ((i != 1) && (i != 3)) ? vector4 : vector3;
                    Vector3 lineEnd = (i != 1) ? ((i != 2) ? vector4 : vector6) : vector5;
                    float num4 = AstarMath.NearestPointFactor(lineStart, lineEnd, target);
                    float positiveInfinity = float.PositiveInfinity;
                    float negativeInfinity = float.NegativeInfinity;
                    bool flag2 = false;
                    for (int j = 0; j < (list.Count - ((i != 3) ? 0 : 1)); j++)
                    {
                        if (drawGizmos)
                        {
                            LocalAvoidance.IntersectionPair pair = list[j];
                            LocalAvoidance.IntersectionPair pair2 = list[j];
                            Debug.DrawRay(lineStart + ((Vector3) ((lineEnd - lineStart) * pair.factor)), Vector3.down, (pair2.state != LocalAvoidance.IntersectionState.Outside) ? Color.red : Color.green);
                        }
                        LocalAvoidance.IntersectionPair pair3 = list[j];
                        if (pair3.state != LocalAvoidance.IntersectionState.Outside)
                        {
                            continue;
                        }
                        if (j == (list.Count - 1))
                        {
                            if (j == 0)
                            {
                                goto Label_02D6;
                            }
                            LocalAvoidance.IntersectionPair pair4 = list[j - 1];
                            if (pair4.state != LocalAvoidance.IntersectionState.Outside)
                            {
                                goto Label_02D6;
                            }
                        }
                        if (j >= (list.Count - 1))
                        {
                            continue;
                        }
                        LocalAvoidance.IntersectionPair pair5 = list[j + 1];
                        if (pair5.state != LocalAvoidance.IntersectionState.Outside)
                        {
                            continue;
                        }
                    Label_02D6:
                        flag2 = true;
                        LocalAvoidance.IntersectionPair pair6 = list[j];
                        float factor = pair6.factor;
                        float num9 = (j != (list.Count - 1)) ? list[j + 1].factor : ((i != 3) ? float.PositiveInfinity : 1f);
                        if (drawGizmos)
                        {
                            Debug.DrawLine((lineStart + ((Vector3) ((lineEnd - lineStart) * factor))) + Vector3.up, (lineStart + ((Vector3) ((lineEnd - lineStart) * Mathf.Clamp01(num9)))) + Vector3.up, Color.green);
                        }
                        if ((factor <= num4) && (num9 >= num4))
                        {
                            positiveInfinity = num4;
                            negativeInfinity = num4;
                            break;
                        }
                        if ((num9 < num4) && (num9 > negativeInfinity))
                        {
                            negativeInfinity = num9;
                        }
                        else if ((factor > num4) && (factor < positiveInfinity))
                        {
                            positiveInfinity = factor;
                        }
                    }
                    if (!flag2)
                    {
                        continue;
                    }
                    float num10 = (positiveInfinity != float.NegativeInfinity) ? ((negativeInfinity != float.PositiveInfinity) ? ((Mathf.Abs((float) (num4 - positiveInfinity)) >= Mathf.Abs((float) (num4 - negativeInfinity))) ? negativeInfinity : positiveInfinity) : positiveInfinity) : negativeInfinity;
                    Vector3 vector9 = lineStart + ((Vector3) ((lineEnd - lineStart) * num10));
                    if (flag)
                    {
                        Vector3 vector10 = vector9 - target;
                        Vector3 vector11 = closest - target;
                        if (vector10.sqrMagnitude >= vector11.sqrMagnitude)
                        {
                            goto Label_04A0;
                        }
                    }
                    closest = vector9;
                Label_04A0:
                    if (drawGizmos)
                    {
                        Debug.DrawLine(target, closest, Color.yellow);
                    }
                    flag = true;
                }
                return flag;
            }

            public static explicit operator LocalAvoidance.HalfPlane(LocalAvoidance.VO vo)
            {
                return new LocalAvoidance.HalfPlane { point = vo.origin + ((Vector3) (vo.direction * vo.limit)), normal = -vo.direction };
            }

            public float ScoreContains(Vector3 p)
            {
                return 0f;
            }
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct VOIntersection
        {
            public LocalAvoidance.VO vo1;
            public LocalAvoidance.VO vo2;
            public float factor1;
            public float factor2;
            public bool inside;
            public VOIntersection(LocalAvoidance.VO vo1, LocalAvoidance.VO vo2, float factor1, float factor2, bool inside = false)
            {
                this.vo1 = vo1;
                this.vo2 = vo2;
                this.factor1 = factor1;
                this.factor2 = factor2;
                this.inside = inside;
            }
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct VOLine
        {
            public LocalAvoidance.VO vo;
            public Vector3 start;
            public Vector3 end;
            public bool inf;
            public int id;
            public bool wrongSide;
            public VOLine(LocalAvoidance.VO vo, Vector3 start, Vector3 end, bool inf, int id, bool wrongSide)
            {
                this.vo = vo;
                this.start = start;
                this.end = end;
                this.inf = inf;
                this.id = id;
                this.wrongSide = wrongSide;
            }
        }
    }
}

